import configparser
import json
import logging
import os
import queue
import random
import sys
from typing import List

from PyQt5.QtCore import QThread, pyqtSignal, QTimer
from PyQt5.QtWidgets import QMessageBox, QApplication
from paho.mqtt import client as mqtt_client

from data.defines import *
from data.recorder import datarecoder

file_path_mac = os.getcwd() + '/'
file_path_win = os.getcwd() + '\\'


def parse_json_str_data(msg):
    try:
        json_data = json.loads(msg.payload.decode())
    except Exception as e:
        logging.error(e.args)  # 访问异常的错误编号和详细信息
        logging.error(str(e))
        logging.error(repr(e))
        json_data = None
    return json_data


def on_connect(client, userdata, flags, rc):
    if rc == 0:
        logging.info("Connected to MQTT Broker!")
    else:
        logging.info("Failed to connect, return code %d\n", rc)


def get_send_cmd_msg(dict_send_cmd):
    msg_str = ""
    try:
        if dict_send_cmd['isControlCmd'] == 1:
            msg_str = COMMAND_NAME_DICT[CommandType.Control][dict_send_cmd['ControlCmd']]

        elif dict_send_cmd['isAdjustCmd'] == 1:
            cmd_idx = dict_send_cmd['AdjustCmd']
            cmd_param = dict_send_cmd['AdjustCmdData']
            msg_str = COMMAND_NAME_DICT[CommandType.Adjust][cmd_idx]

            if cmd_idx == COMMAND_ADJUST_TAXI_OUT_ROUTE_UPLOAD or cmd_idx == COMMAND_ADJUST_TAXI_IN_ROUTE_UPLOAD:
                msg_str += " "
                msg_str += str(dict_send_cmd['taxi_way_dot'])

            elif cmd_idx == COMMAND_ADJUST_ROUTE_SET:
                msg_str += " "
                msg_str += str(dict_send_cmd['on_way_dot'])
            elif cmd_idx == COMMAND_ADJUST_ADC2_HEATING:
                list_str = [" 自动加热", " 强制关闭", " 强制开启"]
                msg_str += list_str[cmd_param]
            elif cmd_idx == COMMAND_ADJUST_ADC2_VIAS_RESET:
                list_str = [" 解锁", " 清零"]
                msg_str += list_str[cmd_param]
            elif cmd_idx == COMMAND_ADJUST_ADC2_AOA_RESET:
                list_str = ["解锁", "迎角0", "迎角1", "侧滑角0", "侧滑角1"]
                msg_str += list_str[cmd_param]
            else:
                # 根据命令类别进行参数特殊处理
                msg_str += " "
                if cmd_idx == COMMAND_ADJUST_AIR_SPEED:
                    msg_str += "%.1f" % (cmd_param * 3.6)
                else:
                    msg_str += str(cmd_param)
                if cmd_idx == COMMAND_ADJUST_ROUTE_DIRECT \
                        or cmd_idx == COMMAND_ADJUST_PITCH \
                        or cmd_idx == COMMAND_ADJUST_ROLL \
                        or cmd_idx == COMMAND_ADJUST_LIFT_RUDDER \
                        or cmd_idx == COMMAND_ADJUST_AILERON_RUDDER \
                        or cmd_idx == COMMAND_ADJUST_COURSE_RUDDER \
                        or cmd_idx == COMMAND_ADJUST_FRONT_WHEEL_DEG \
                        or cmd_idx == COMMAND_ADJUST_TRACE_ANGLE:
                    msg_str += "°"
                elif cmd_idx == COMMAND_ADJUST_FIELD_HEIGHT \
                        or cmd_idx == COMMAND_ADJUST_YAW \
                        or cmd_idx == COMMAND_ADJUST_ROUTE_ALT:
                    msg_str += "m"
                elif cmd_idx == COMMAND_ADJUST_THR \
                        or cmd_idx == COMMAND_ADJUST_BACK_WHEEL_DIFF \
                        or cmd_idx == COMMAND_ADJUST_BACK_WHEEL_BRAKE \
                        or cmd_idx == COMMAND_ADJUST_LOWSPACE_FLY_ALT:
                    msg_str += "%"
                elif cmd_idx == COMMAND_ADJUST_AIR_SPEED:
                    msg_str += "km/h"
    except Exception as e:
        logging.error(e.args)  # 访问异常的错误编号和详细信息
        logging.error(str(e))
        logging.error(repr(e))

    return msg_str


def get_expect_code(sent_cmd):
    except_remote_code = -1
    if sent_cmd['isControlCmd'] == 1:
        except_remote_code = CTRL_COMMAND_BACK_DICT[sent_cmd['ControlCmd']]

    elif sent_cmd['isAdjustCmd'] == 1:
        except_remote_code = ADJUST_ECHO_CODE_LIST[sent_cmd['AdjustCmd']]
    return except_remote_code


class DataHandler(QThread):
    # _has_init = False
    record_on = True
    # 信号定义，用于在主界面中监控响应的事件并更新UI
    sig_RecvServoData = pyqtSignal(object)  # 收到舵系统数据的信号源
    sig_RecvFccSystmData = pyqtSignal(object)  # 接收到飞控系统信息
    sig_RecvSensorINS = pyqtSignal(object)  # 接收到惯导信息
    sig_RecvSensorBINS = pyqtSignal(object)  # 接收到备用惯导信息
    sig_RecvSensorADC = pyqtSignal(object)  # 接收到大气机信息
    sig_RecvSensorADC2 = pyqtSignal(object)  # 接收到备用大气机信息
    sig_RecvSensorGPS = pyqtSignal(object)  # 接收到GPS信息
    sig_RecvSensorRadar = pyqtSignal(object)  # 接收到雷达信息
    sig_RecvDataLink = pyqtSignal(object)  # 接收到数据链信息
    sig_RecvElecSystem = pyqtSignal(object)  # 接收到电气系统信息
    sig_RecvEngineSystem = pyqtSignal(object)  # 接收到发动机系统信息
    sig_RecvWarningSystem = pyqtSignal(object)  # 接收到告警系统信息
    sig_RecvEquipment = pyqtSignal(object)  # 接收到其他设备信息
    sig_RecvCommandEcho = pyqtSignal(object)  # 接收到指令回传信息

    sig_pre_send_request = pyqtSignal(object)  # 预备发送命令信号 用于由其他页面检测并更新提示信息
    sig_send_request = pyqtSignal(object)  # 发送命令信号，用于更新主界面已发送命令和操作记录
    sig_request_echo = pyqtSignal(object)  # 用于检测命令返回状态的信号

    # 航线相关信号
    sig_online_route_download_cmp = pyqtSignal(list)  # 用于发送在线航线查询结果的信号
    sig_route_download_cmp = pyqtSignal(list)  # 用于发送航线查询结果的信号
    sig_taxiin_route_download_cmp = pyqtSignal(list)
    sig_taxiout_route_download_cmp = pyqtSignal(list)

    # 用于接收外部事件的信号
    sig_RemoteCtrlCmd = pyqtSignal(object)

    # TODO:将所有的topic存储为变量，在创建对象时根据配置文件初始化
    pub_topic_command_send = "/STAR650/1/1.0/ULINK/command_send"
    sub_topic_servo_system = "/STAR650/1/1.0/ULINK/servo_system"
    sub_topic_fcc_system = "/STAR650/1/1.0/ULINK/fcc_system"
    sub_topic_sensor_ins = "/STAR650/1/1.0/ULINK/sensor_ins"
    sub_topic_sensor_bins = "/STAR650/1/1.0/ULINK/sensor_bins"
    sub_topic_sensor_adc = "/STAR650/1/1.0/ULINK/sensor_adc"
    sub_topic_sensor_adc2 = "/STAR650/1/1.0/ULINK/sensor_adc2"
    sub_topic_sensor_radar = "/STAR650/1/1.0/ULINK/sensor_radar"
    sub_topic_sensor_gps = "/STAR650/1/1.0/ULINK/sensor_gps"
    sub_topic_datalink = "/STAR650/1/1.0/ULINK/datalink"
    sub_topic_elec_system = "/STAR650/1/1.0/ULINK/elec_system"
    sub_topic_engine_system = "/STAR650/1/1.0/ULINK/engine_system"
    sub_topic_warnning_system = "/STAR650/1/1.0/ULINK/warnning_system"
    sub_topic_equipment = "/STAR650/1/1.0/ULINK/equipment"
    sub_topic_command_echo = "/STAR650/1/1.0/ULINK/command_echo"
    mqtt_broker_address = "127.0.0.1"
    mqtt_port = 1883

    def __init__(self):  # 定义对象时初始化mqtt客户端
        # if not DataHandler._has_init:
        # DataHandler._has_init = True

        super().__init__()
        self.elec_adc_heat_cmd = -1  # 空速管加温控制状态指示 -1代表未初始化，0-关闭 1-开启
        self.request_result = []
        self.connect_status = False  # 用于判断地面站和飞控连接状态的标志变量
        self.recv_msg_counter = 0  # 用于计算接收数据的计数器变量
        self.recv_msg_counter_old = 0

        self.takeoff_check_finished = False  # 用于判断飞行前检查是否完成的标志变量，完成检查后将该变量置为true

        self.counter_upload_fail = 0
        self.get_complete_dot_cnt = 0
        self.last_AdjustCmdData = -1
        self.send_route_enable = 0
        self.counter_send_dot_fail_retry = 0  # 上传失败计数器，当计数超过5次则停止上传
        self.upload_route_type = 0  # 上传航线类型 0-在线航点 1-滑行驶入航点 2-滑行驶出航点
        self.request_route_type = 0  # 下载航线类型 0-在线航点 1-滑行驶入航点 2-滑行驶出航点
        self.list_route_to_send = []

        self._orig_pos_data_read()  # 读取配置文件，获取配置文件中机场位置位置数据
        self._gcs_config_load()  # 读取配置文件，获取mqtt服务器相关配置信息
        self._mqtt_init()  # 读取配置文件，初始化mqtt客户端
        self._signal_event_init()  # 初始化信号响应事件

        self.send_timer = QTimer()
        self.send_timer.timeout.connect(self._send_request_5Hzprocess)
        self.send_timer.setInterval(200)

        self.read_timer = QTimer()
        self.read_timer.timeout.connect(self._read_timer_timeout)
        self.read_timer.setInterval(1000)

        self.request_queue = queue.Queue(50)
        self.timout_request_queue = queue.Queue(50)
        self.timeout_counter = 0  # 回传检测超时判断变量 默认为0，在回传检测中进行自增运算，超过15后置零并触发超时

        self.start()

    def _orig_pos_data_save(self):
        current = os.getcwd()
        if current.startswith('/'):
            filename = file_path_mac + runway_set_filename
        else:
            filename = file_path_win + runway_set_filename
        if os.path.exists(filename):
            cf = configparser.ConfigParser()
            try:
                cf.read(filename)
                if not cf.has_section("ORIGIONAL_POS"):
                    cf.add_section("ORIGIONAL_POS")
                cf.set("ORIGIONAL_POS", "START_LON", str(self.origional_lon))
                cf.set("ORIGIONAL_POS", "START_LAT", str(self.origional_lat))
                cf.write(open(filename, "w"))
            except BaseException as e:
                logging.error(repr(e))

    def _orig_pos_data_read(self):
        """
        根据配置文件初始化原点位置，用于计算相对位置信息
        :return:
        """
        current = os.getcwd()
        if current.startswith('/'):
            filename = file_path_mac + runway_set_filename
        else:
            filename = file_path_win + runway_set_filename
        if os.path.exists(filename):
            cf = configparser.ConfigParser()
            try:
                self.origional_pos_inited = False
                self.origional_lon = 0
                self.origional_lat = 0

                cf.read(filename)
                start_lon = cf.get("ORIGIONAL_POS", "START_LON", fallback=None)
                start_lat = cf.get("ORIGIONAL_POS", "START_LAT", fallback=None)
                # 此处获取的是字符串变量，使用时需要转换为浮点数
                if (start_lon is not None) and (start_lat is not None):
                    self.origional_pos_inited = True
                    self.origional_lon = float(start_lon)
                    self.origional_lat = float(start_lat)
                    logging.info("获取缓存初始位置")
                else:
                    start_lon = cf.get(FILE_LEVEL_RUNWAY, FILE_ITEM_RUNWAY_START_LON, fallback=None)
                    start_lat = cf.get(FILE_LEVEL_RUNWAY, FILE_ITEM_RUNWAY_START_LAT, fallback=None)
                    if (start_lon is not None) and (start_lat is not None):
                        self.origional_pos_inited = True
                        self.origional_lon = float(start_lon)
                        self.origional_lat = float(start_lat)
                        logging.info("未获取缓存初始位置，使用跑道起点作为初始位置")
            except BaseException as e:
                logging.error(repr(e))
                self.origional_pos_inited = False
                self.origional_lon = 0
                self.origional_lat = 0
                logging.warning("地面站参考初始位置未设置")

    def _gcs_config_load(self):
        """
        根据配置文件获取mqtt服务器相关配置信息
        :return:
        """
        current = os.getcwd()
        if current.startswith('/'):
            filename = file_path_mac + port_filename
        else:
            filename = file_path_win + port_filename
        if os.path.exists(filename):
            cf = configparser.ConfigParser()
            try:
                cf.read(filename)
                self.mqtt_broker_address = cf.get(NODE_SETTING, ITEM_MQTT_SERVER_IP, fallback="127.0.0.1").strip(
                    '"').strip("'").strip()
                self.mqtt_port = cf.getint(NODE_SETTING, ITEM_MQTT_PORT, fallback=1883)

                self.pub_topic_command_send = cf.get(NODE_PUB_TOPIC, ITEM_COMMAND_SEND,
                                                     fallback="/STAR650/1/1.0/ULINK/command_send").strip(
                    "'").strip().strip('"').strip("'").strip()

                self.sub_topic_servo_system = cf.get(NODE_SUB_TOPIC, ITEM_SERVO,
                                                     fallback="/STAR650/1/1.0/ULINK/servo_system").strip('"').strip(
                    "'").strip()

                self.sub_topic_fcc_system = cf.get(NODE_SUB_TOPIC, ITEM_FCC,
                                                   fallback="/STAR650/1/1.0/ULINK/fcc_system").strip('"').strip(
                    "'").strip()
                self.sub_topic_sensor_ins = cf.get(NODE_SUB_TOPIC, ITEM_INS,
                                                   fallback="/STAR650/1/1.0/ULINK/sensor_ins").strip('"').strip(
                    "'").strip()
                self.sub_topic_sensor_bins = cf.get(NODE_SUB_TOPIC, ITEM_BINS,
                                                    fallback="/STAR650/1/1.0/ULINK/sensor_bins").strip('"').strip(
                    "'").strip()
                self.sub_topic_sensor_adc = cf.get(NODE_SUB_TOPIC, ITEM_ADC,
                                                   fallback="/STAR650/1/1.0/ULINK/sensor_adc").strip('"').strip(
                    "'").strip()
                self.sub_topic_sensor_adc2 = cf.get(NODE_SUB_TOPIC, ITEM_ADC2,
                                                    fallback="/STAR650/1/1.0/ULINK/sensor_adc2").strip('"').strip(
                    "'").strip()
                self.sub_topic_sensor_radar = cf.get(NODE_SUB_TOPIC, ITEM_RADAR,
                                                     fallback="/STAR650/1/1.0/ULINK/sensor_radar").strip('"').strip(
                    "'").strip()
                self.sub_topic_sensor_gps = cf.get(NODE_SUB_TOPIC, ITEM_GPS,
                                                   fallback="/STAR650/1/1.0/ULINK/sensor_gps").strip('"').strip(
                    "'").strip()
                self.sub_topic_datalink = cf.get(NODE_SUB_TOPIC, ITEM_DATALINK,
                                                 fallback="/STAR650/1/1.0/ULINK/datalink").strip('"').strip("'").strip()
                self.sub_topic_elec_system = cf.get(NODE_SUB_TOPIC, ITEM_ELEC,
                                                    fallback="/STAR650/1/1.0/ULINK/elec_system").strip('"').strip(
                    "'").strip()
                self.sub_topic_engine_system = cf.get(NODE_SUB_TOPIC, ITEM_ENGINE,
                                                      fallback="/STAR650/1/1.0/ULINK/engine_system").strip('"').strip(
                    "'").strip()
                self.sub_topic_warnning_system = cf.get(NODE_SUB_TOPIC, ITEM_WARNNING,
                                                        fallback="/STAR650/1/1.0/ULINK/warnning_system").strip(
                    '"').strip("'").strip()
                self.sub_topic_equipment = cf.get(NODE_SUB_TOPIC, ITEM_EQUIPMENT,
                                                  fallback="/STAR650/1/1.0/ULINK/equipment").strip('"').strip(
                    "'").strip()
                self.sub_topic_command_echo = cf.get(NODE_SUB_TOPIC, ITEM_ECHO_CMD,
                                                     fallback="/STAR650/1/1.0/ULINK/command_echo").strip('"').strip(
                    "'").strip()

                # 此处获取的是字符串变量，使用时需要转换为浮点数

            except BaseException as e:
                logging.error(repr(e))
                logging.error("读取端口配置文件失败！")
                replay = QMessageBox.warning(None, "警告", "读取地面站配置文件失败，请检查配置文件是否完好！",
                                             QMessageBox.Yes, QMessageBox.Yes)

    def timer_init(self):
        """
        初始化发送定时器
        """
        self.send_timer.start()
        self.read_timer.start()

    def upload_switch(self, upload_switch):
        if upload_switch == 0:
            self.send_timer.stop()
        else:
            self.send_timer.start()

    def direct_send_request(self, ctrl_adjust_type, cmd_idx, cmd_params=None):
        cmd_send = self.pre_send_request_cmd(ctrl_adjust_type, cmd_idx, cmd_params)
        self.send_request(cmd_send)

    def pre_send_request_cmd(self, ctrl_adjust_type, cmd_idx, cmd_params=None):
        """
        预备发送命令，传入命令类型，命令编号和命令参数，获取组织好的DataHandler，然后通过发送按钮调用发送接口进行命令发送

        :param ctrl_adjust_type: 输入命令类型 0-遥控命令，1-遥调命令
        :param cmd_idx: 命令编号，分为遥控和遥调命令，具体取值范围见表格
        :param cmd_params:遥调命令参数 遥控指令对应空参数，普通遥调指令对应一个浮点数或整数，
                            滑行航线装订时对应数据结构为：
                            {"on_way_dot": 0,  # 航点号
                                "on_way_lon": 0,  # 航点经度
                                "on_way_lat": 0,  # 航点纬度
                                "on_way_alt": 0,  # 航点高度
                                "on_way_vxd": 0,  # 航点任务字
                            }
                            在线航线状态时对应数据结构为：
                            {"taxi_way_dot": 0,  # 航点号
                                "taxi_way_lon": 0,  # 航点经度
                                "taxi_way_lat": 0,  # 航点纬度
                                "taxi_way_radious": 0,  # 转弯半径
                            }
        :return:
        """
        cmd_send = {
            "CmdType": 1,  # 1 - 代表地面站遥控指令 2 - 代表地检软件遥控指令
            "isControlCmd": 0,  # 当前指令是否为遥控指令 1 - 遥控指令，0 - 非遥控指令
            "ControlCmd": 0,  # 遥控指令编号，见协议中遥控指令表格
            "isAdjustCmd": 0,  # 当前指令是否为遥调指令1 - 遥调指令，0 - 非遥调指令
            "AdjustCmd": 0,  # 遥调指令编号，见协议中遥调指令表格
            "AdjustCmdData": 0.0,  # 遥调指令数据，见协议中遥调指令表格

            # 在线航线上传数据
            "on_way_dot": 0,  # 航点号
            "on_way_lon": 0,  # 航点经度
            "on_way_lat": 0,  # 航点纬度
            "on_way_alt": 0,  # 航点高度
            "on_way_vxd": 0,  # 航点任务字

            # 驶入驶出航线上传数据
            "taxi_way_dot": 0,  # 航点号
            "taxi_way_lon": 0,  # 航点经度
            "taxi_way_lat": 0,  # 航点纬度
            "taxi_way_radious": 0,  # 转弯半径
            "RemoteCode": 0  # 回传指令，用于和下行数据中的回传指令对比
        }
        msg_str = "无效命令"
        # 根据cmd_idx获取指令，并触发界面更新信号用于UI更新
        if ctrl_adjust_type == 0:
            if cmd_idx in COMMAND_NAME_DICT[CommandType.Control]:
                cmd_send['isControlCmd'] = 1
                cmd_send['ControlCmd'] = cmd_idx
                if cmd_idx == COMMAND_CTRL_ANGLE_OF_TAKEOFF:
                    # 检测到预起飞指令时，保存当前位置作为起飞原点
                    sensor_fusion = self.fcc_system['sensor_fusion']
                    self.origional_pos_inited = True
                    self.origional_lon = sensor_fusion['lon_deg']
                    self.origional_lat = sensor_fusion['lat_deg']
                    self._orig_pos_data_save()
        else:
            if cmd_idx in COMMAND_NAME_DICT[CommandType.Adjust]:
                cmd_send['isAdjustCmd'] = 1
                cmd_send['AdjustCmd'] = cmd_idx
                if cmd_idx == COMMAND_ADJUST_TAXI_OUT_ROUTE_UPLOAD or cmd_idx == COMMAND_ADJUST_TAXI_IN_ROUTE_UPLOAD:
                    # 从param中获取数据拷贝到滑行航线装订数据中
                    try:
                        cmd_send['taxi_way_dot'] = cmd_params['taxi_way_dot']
                        cmd_send['taxi_way_lon'] = cmd_params['taxi_way_lon']
                        cmd_send['taxi_way_lat'] = cmd_params['taxi_way_lat']
                        cmd_send['taxi_way_radious'] = cmd_params['taxi_way_radious']

                    except Exception as e:
                        logging.error(e.args)  # 访问异常的错误编号和详细信息
                        logging.error(str(e))
                        logging.error(repr(e))
                elif cmd_idx == COMMAND_ADJUST_ROUTE_SET:
                    # 从param中获取数据拷贝到在线航线装订数据中
                    try:
                        cmd_send['on_way_dot'] = cmd_params['on_way_dot']
                        cmd_send['on_way_lon'] = cmd_params['on_way_lon']
                        cmd_send['on_way_lat'] = cmd_params['on_way_lat']
                        cmd_send['on_way_alt'] = cmd_params['on_way_alt']
                        cmd_send['on_way_vxd'] = cmd_params['on_way_vxd']
                    except Exception as e:
                        logging.error(e.args)  # 访问异常的错误编号和详细信息
                        logging.error(str(e))
                        logging.error(repr(e))
                elif cmd_idx == COMMAND_ADJUST_TAXI_IN \
                        or cmd_idx == COMMAND_ADJUST_TAXI_OUT \
                        or cmd_idx == COMMAND_ADJUST_ROUTE_NUM:
                    # cmd_send['AdjustCmdData'] = cmd_params[0]
                    # TODO:待更新上行协议，同时传入航路编号和航点号用于上行命令发送
                    cmd_send['AdjustCmdData'] = cmd_params[1]
                elif cmd_idx == COMMAND_ADJUST_AIR_SPEED:
                    # 空速遥调时，将输入值单位更改为m/s后发送
                    cmd_send['AdjustCmdData'] = cmd_params / 3.6

                else:
                    # 根据命令类别进行参数特殊处理
                    cmd_send['AdjustCmdData'] = cmd_params
                    # print("cmd_params = " + str(cmd_params))

        msg_str = get_send_cmd_msg(cmd_send)
        # print("预备发送命令：" + msg_str)
        # print(cmd_send)

        self.cmd_to_send_msg = msg_str  # 更新待发送命令的提示信息
        self.cmd_to_send = cmd_send  # 更新待发送的命令

        self.sig_pre_send_request.emit(msg_str)  # 预备发送命令信号更新

        return cmd_send

    def _read_timer_timeout(self):
        if self.recv_msg_counter_old == self.recv_msg_counter:
            self.connect_status = False
            if datarecoder.working:
                datarecoder.stop_record()
            logging.error("连接丢失")
        else:
            self.connect_status = True
            if not datarecoder.working:
                datarecoder.start_record()
            # logging.info("连接成功")

        self.recv_msg_counter_old = self.recv_msg_counter

    def _send_request_5Hzprocess(self):
        """
        定时器事件到了之后执行mqtt消息发送
        从发送队列中取出待发送指令，如果存在有效指令则执行发送，如果不存在则发送空指令
        将发送的指令加入超时队列，并开启反馈值检测定时器
        :return:
        """
        cmd_send = {
            "CmdType": 1,  # 1 - 代表地面站遥控指令 2 - 代表地检软件遥控指令
            "isControlCmd": 0,  # 当前指令是否为遥控指令 1 - 遥控指令，0 - 非遥控指令
            "ControlCmd": 0,  # 遥控指令编号，见协议中遥控指令表格
            "isAdjustCmd": 0,  # 当前指令是否为遥调指令1 - 遥调指令，0 - 非遥调指令
            "AdjustCmd": 0,  # 遥调指令编号，见协议中遥调指令表格
            "AdjustCmdData": 0.0,  # 遥调指令数据，见协议中遥调指令表格

            # 在线航线上传数据
            "on_way_dot": 0,  # 航点号
            "on_way_lon": 0,  # 航点经度
            "on_way_lat": 0,  # 航点纬度
            "on_way_alt": 0,  # 航点高度
            "on_way_vxd": 0,  # 航点任务字

            # 驶入驶出航线上传数据
            "taxi_way_dot": 0,  # 航点号
            "taxi_way_lon": 0,  # 航点经度
            "taxi_way_lat": 0,  # 航点纬度
            "taxi_way_radious": 0,  # 转弯半径
            "RemoteCode": 0  # 回传指令，用于和下行数据中的回传指令对比
        }

        if self.request_queue.empty():
            # 发送空指令
            send_msg = json.dumps(cmd_send)
        else:
            send_msg = json.dumps(self.request_queue.get())

        datarecoder.command_send = cmd_send

        self.publish_message(self.pub_topic_command_send, send_msg)

        # 根据超时队列中的数据进行回传数据检测
        if self.timout_request_queue.empty():
            self.timeout_counter = 0  # 当前没有需要检测回传数据的命令
        else:  # 队列中存在待检测反馈值的命令
            try:
                sent_cmd = self.timout_request_queue.get()  # 获取待检测的命令
                echo_remote_code = self.command_echo['echo_remote_code']  # 从接收到的数据中获取RemoteCode反馈值
                except_remote_code = get_expect_code(sent_cmd)  # 计算期望反馈值

                # 如果接收到和发送命令匹配的反馈值
                if echo_remote_code == except_remote_code:
                    self.timeout_counter = 0  # 检测到正确的反馈值，命令发送成功
                    # 命令发送成功信号激活
                    msg_str = get_send_cmd_msg(sent_cmd) + "成功"  # 根据发送的命令获取要显示的字符串
                    self.sig_request_echo.emit(msg_str)

                    # 在线航线上传命令处理
                    if echo_remote_code == BACK_ADJUST_ROUTE_SET:
                        # 如果是在线航线加载命令，则检查是否还有剩余航线并继续发送加载指令
                        if self.send_route_enable == 1:
                            # 对比上传数据和返回的数据是否一致
                            tmp_vxd = self.list_route_to_send[0].status << 12 + self.list_route_to_send[0].parm
                            if self.command_echo["way_dot_num"] == self.list_route_to_send[0].no and \
                                    abs(self.command_echo["way_dot_lon"] - self.list_route_to_send[
                                        0].lon) < 0.000001 and \
                                    abs(self.command_echo["way_dot_lat"] - self.list_route_to_send[
                                        0].lat) < 0.000001 and \
                                    abs(self.command_echo["way_dot_alt"] - self.list_route_to_send[
                                        0].alt) < 0.000001 and \
                                    self.command_echo["way_dot_vxd"] == tmp_vxd:
                                # 上传点和下载点数据一致，删除当前点，进行下一个点的上传
                                self.counter_upload_fail = 0
                                self.list_route_to_send.pop(0)
                            else:  # 计算同一个点的上传次数，如果上传次数超过阈值，则停止上传并提示失败信息
                                self.counter_upload_fail += 1
                        if self.counter_upload_fail < 5:  # 如果错误计数器次数超过5则停止上传
                            if len(self.list_route_to_send) > 0:
                                self.counter_send_dot_fail_retry = 0
                                self._send_next_dot(self.upload_route_type)  # 上传下一个点
                            else:
                                self.send_route_enable = 0  # 上传队列为空，完成上传关闭上传开始标志
                                msg_str = "航线上传完成"
                                replay = QMessageBox.information(None, "通知", "航线上传成功！",
                                                                 QMessageBox.Yes, QMessageBox.Yes)
                                self.sig_request_echo.emit(msg_str)
                        else:
                            self.send_route_enable = 0  # 关闭上传标志
                            msg_str = "航线上传失败"
                            replay = QMessageBox.information(None, "警告", "航线上传失败！",
                                                             QMessageBox.Yes, QMessageBox.Yes)
                            self.sig_request_echo.emit(msg_str)

                    if echo_remote_code == BACK_ADJUST_TAXI_IN_ROUTE_UPLOAD \
                            or echo_remote_code == BACK_ADJUST_TAXI_OUT_ROUTE_UPLOAD:
                        if self.send_route_enable == 1:
                            # 对比上传数据和返回的数据是否一致
                            if self.command_echo["way_dot_num"] == self.list_route_to_send[0].no \
                                    and abs(
                                self.command_echo["way_dot_lon"] - self.list_route_to_send[0].lon) < 0.000001 \
                                    and abs(
                                self.command_echo["way_dot_lat"] - self.list_route_to_send[0].lat) < 0.000001 \
                                    and abs(
                                self.command_echo["way_dot_radious"] - self.list_route_to_send[0].radious) < 0.01:
                                # 上传点和下载点数据一致，删除当前点，进行下一个点的上传
                                self.counter_upload_fail = 0
                                self.list_route_to_send.pop(0)
                            else:  # 计算同一个点的上传次数，如果上传次数超过阈值，则停止上传并提示失败信息
                                self.counter_upload_fail += 1

                        if self.counter_upload_fail < 5:  # 如果错误计数器次数超过5则停止上传
                            if len(self.list_route_to_send) > 0:
                                self.counter_send_dot_fail_retry = 0
                                self._send_next_dot(self.upload_route_type)  # 上传下一个点
                            else:
                                self.send_route_enable = 0  # 上传队列为空，完成上传关闭上传开始标志
                                msg_str = "航线上传完成"
                                replay = QMessageBox.information(None, "通知", "航线上传成功！",
                                                                 QMessageBox.Yes, QMessageBox.Yes)
                                self.sig_request_echo.emit(msg_str)
                        else:
                            self.send_route_enable = 0  # 关闭上传标志
                            msg_str = "航线上传失败"
                            replay = QMessageBox.information(None, "警告", "航线上传失败！",
                                                             QMessageBox.Yes, QMessageBox.Yes)
                            self.sig_request_echo.emit(msg_str)

                    # 在线航线下载命令处理
                    if echo_remote_code == BACK_ADJUST_ROUTE_BACK \
                            or echo_remote_code == BACK_ADJUST_TAXI_IN_ROUTE_DOWNLOAD \
                            or echo_remote_code == BACK_ADJUST_TAXI_OUT_ROUTE_DOWNLOAD:  # 查询到在线航点数据结果
                        if self.last_AdjustCmdData == sent_cmd['AdjustCmdData'] and \
                                self.command_echo["way_dot_num"] == 0:  # 查询到航线最后一个点
                            self.get_complete_dot_cnt += 1

                        if self.get_complete_dot_cnt > 3:  # 查询已完成
                            logging.info("在线航线下载完成")
                            self.get_complete_dot_cnt = 0
                            # 将航线数据发送到查询表格中（创建航线查询成功信号，通过该信号发送查询结果）
                            if self.request_route_type == 0:
                                self.sig_online_route_download_cmp.emit(self.request_result)
                            elif self.request_route_type == 1:  # 驶入航线
                                self.sig_taxiin_route_download_cmp.emit(self.request_result)
                            elif self.request_route_type == 2:  # 驶出航线
                                self.sig_taxiout_route_download_cmp.emit(self.request_result)

                        else:  # 查询进行中，继续发送查询命令
                            if sent_cmd['AdjustCmdData'] == self.command_echo["way_dot_num"]:
                                # 当前点查询成功，保存数据并查询下一个点
                                # 存储单个点的信息到结果队列中
                                tmp = RouteInfo()
                                tmp.no = self.command_echo["way_dot_num"]
                                tmp.lon = self.command_echo["way_dot_lon"]
                                tmp.lat = self.command_echo["way_dot_lat"]
                                tmp.alt = self.command_echo["way_dot_alt"]
                                tmp.spd = 0
                                vxd = self.command_echo["way_dot_vxd"]
                                tmp.status = (vxd & 0xF000) >> 12
                                tmp.parm = vxd & 0x000F
                                tmp.radious = self.command_echo['way_dot_radious']
                                self.request_result.append(tmp)
                                # 发送查询下一个点的命令
                                self.request_online_route_data(sent_cmd['AdjustCmdData'] + 1, self.request_route_type)

                            else:  # 还未查询到当前点的数据，继续发送查询当前点的命令
                                self.request_online_route_data(sent_cmd['AdjustCmdData'], self.request_route_type)
                        self.last_AdjustCmdData = sent_cmd['AdjustCmdData']

                    # 保存ADC加热控制指令状态，用于检查反馈状态
                    if echo_remote_code == BACK_CTRL_OUTPUT_B_CLOSE:
                        self.elec_adc_heat_cmd = 0
                    if echo_remote_code == BACK_CTRL_OUTPUT_B_OPEN:
                        self.elec_adc_heat_cmd = 1

                else:  # 获取的反馈指令和发送指令不一致，可能是指令未响应或反馈指令未更新
                    # 进行超时计数
                    self.timeout_counter += 1
                    if self.timeout_counter < 15:
                        self.timout_request_queue.put(sent_cmd)
                    else:  # 超时计数超过15次后提示命令发送失败
                        self.timeout_counter = 0
                        msg_str = get_send_cmd_msg(sent_cmd) + "失败"
                        self.sig_request_echo.emit(msg_str)  # 超时，发送命令失败提示

                        if except_remote_code == BACK_ADJUST_ROUTE_SET \
                                or except_remote_code == BACK_ADJUST_TAXI_IN_ROUTE_UPLOAD \
                                or except_remote_code == BACK_ADJUST_TAXI_OUT_ROUTE_UPLOAD:  # 如果是发送航点失败，则中止航线加载
                            # 如果当前处于在线航路加载状态，则检查是否还有剩余航线并重试航点加载
                            if self.send_route_enable == 1:
                                if self.counter_send_dot_fail_retry <= 2:
                                    if len(self.list_route_to_send) > 0:
                                        self.counter_send_dot_fail_retry += 1
                                        self._send_next_dot(self.upload_route_type)  # 重试一次上传
                                else:  # 重试第三次，停止上传并提示上传失败
                                    self.send_route_enable = 0
                                    msg_str = "在线航线上传失败"
                                    replay = QMessageBox.information(None, "警告", "在线航线上传失败",
                                                                     QMessageBox.Yes, QMessageBox.Yes)
                                    self.sig_request_echo.emit(msg_str)

            except Exception as e:
                logging.error(repr(e))

    def __on_recv_mqtt_msg(self, client, userdata, msg):
        if self.record_on:
            if datarecoder.csv_rec_file is None and datarecoder.rec_orign_file is None:
                datarecoder.start_record()

        self.recv_msg_counter += 1
        # print(f"Received `{msg.payload.decode()}` from `{msg.topic}` topic") # 打印信息示例
        # 收到特定topic数据后触发特定信号，从而更新UI界面内容
        if msg.topic.endswith('servo_system'):
            self.servo_system = parse_json_str_data(msg)
            self.sig_RecvServoData.emit(self.servo_system)  # 触发舵系统数据接收信号 主界面可根据此信号更新UI
            datarecoder.servo_system = self.servo_system

        elif msg.topic.endswith('fcc_system'):
            self.fcc_system = parse_json_str_data(msg)
            self.sig_RecvFccSystmData.emit(self.fcc_system)
            datarecoder.fcc_system = self.fcc_system

        elif msg.topic.endswith('sensor_ins'):
            self.sensor_ins = parse_json_str_data(msg)
            self.sig_RecvSensorINS.emit(self.sensor_ins)
            datarecoder.sensor_ins = self.sensor_ins

        elif msg.topic.endswith('sensor_bins'):
            self.sensor_bins = parse_json_str_data(msg)
            self.sig_RecvSensorBINS.emit(self.sensor_bins)
            datarecoder.sensor_bins = self.sensor_bins

        elif msg.topic.endswith('sensor_adc'):
            self.sensor_adc = parse_json_str_data(msg)
            self.sig_RecvSensorADC.emit(self.sensor_adc)
            datarecoder.sensor_adc = self.sensor_adc

        elif msg.topic.endswith('sensor_adc2'):
            self.sensor_adc2 = parse_json_str_data(msg)
            self.sig_RecvSensorADC2.emit(self.sensor_adc2)
            datarecoder.sensor_adc2 = self.sensor_adc2

        elif msg.topic.endswith('sensor_radar'):
            self.sensor_radar = parse_json_str_data(msg)
            self.sig_RecvSensorRadar.emit(self.sensor_radar)
            datarecoder.sensor_radar = self.sensor_radar

        elif msg.topic.endswith('sensor_gps'):
            self.sensor_gps = parse_json_str_data(msg)
            self.sig_RecvSensorGPS.emit(self.sensor_gps)
            datarecoder.sensor_gps = self.sensor_gps

        elif msg.topic.endswith('datalink'):
            self.datalink = parse_json_str_data(msg)
            self.sig_RecvDataLink.emit(self.datalink)
            datarecoder.datalink = self.datalink

        elif msg.topic.endswith('elec_system'):
            self.elec_system = parse_json_str_data(msg)
            self.sig_RecvElecSystem.emit(self.elec_system)
            datarecoder.elec_system = self.elec_system

        elif msg.topic.endswith('engine_system'):
            self.engine_system = parse_json_str_data(msg)
            self.sig_RecvEngineSystem.emit(self.engine_system)
            datarecoder.engine_system = self.engine_system

        elif msg.topic.endswith('warnning_system'):
            self.warnning_system = parse_json_str_data(msg)
            self.sig_RecvWarningSystem.emit(self.warnning_system)
            datarecoder.warnning_system = self.warnning_system

        elif msg.topic.endswith('equipment'):
            self.equipment = parse_json_str_data(msg)
            self.sig_RecvEquipment.emit(self.equipment)
            datarecoder.equipment = self.equipment

        elif msg.topic.endswith('command_echo'):
            self.command_echo = parse_json_str_data(msg)
            self.sig_RecvCommandEcho.emit(self.command_echo)
            datarecoder.command_echo = self.command_echo

    # mqtt客户端初始化
    def _mqtt_init(self):
        """
        创建mqtt客户端，然后订阅所需的TOPIC
        :return:
        """
        self.mqtt_broker_address = "127.0.0.1"
        self.mqtt_port = 1883
        client_id = 'gcs-mqtt-client-%d' % (random.randint(0, 100))
        self.client = mqtt_client.Client(client_id)
        self.client.on_connect = on_connect
        try:
            self.client.connect(self.mqtt_broker_address, self.mqtt_port)
        except Exception as e:
            # logging.error(e.args)  # 访问异常的错误编号和详细信息
            # logging.error(str(e))
            logging.error(repr(e))
            print("mqtt connect failed!")
            app = QApplication(sys.argv)
            title_str = "MQTT服务器不可用"
            info_str = "当前MQTT服务器（%s:%d）不可用，是否重试？" % (self.mqtt_broker_address, self.mqtt_port)
            replay = QMessageBox.information(None, title_str, info_str,
                                             QMessageBox.No |
                                             QMessageBox.Yes, QMessageBox.Yes)
            retry_counter = 0
            while replay == QMessageBox.Yes:
                retry_counter += 1
                try:
                    self.client.connect(self.mqtt_broker_address, self.mqtt_port)
                except Exception as e:
                    replay = QMessageBox.information(None, title_str, info_str + repr(e),
                                                     QMessageBox.No |
                                                     QMessageBox.Yes, QMessageBox.Yes)
                if retry_counter > 5:
                    break
            if replay == QMessageBox.No or retry_counter >= 5:
                sys.exit()

        # TODO:根据配置信息订阅地面站所需的topic消息
        # 可配置信息：机型、无人机ID、协议版本号、数据通道
        self.client.subscribe(self.sub_topic_servo_system)
        self.client.subscribe(self.sub_topic_fcc_system)
        self.client.subscribe(self.sub_topic_sensor_ins)
        self.client.subscribe(self.sub_topic_sensor_bins)
        self.client.subscribe(self.sub_topic_sensor_adc)
        self.client.subscribe(self.sub_topic_sensor_adc2)
        self.client.subscribe(self.sub_topic_sensor_radar)
        self.client.subscribe(self.sub_topic_sensor_gps)
        self.client.subscribe(self.sub_topic_datalink)
        self.client.subscribe(self.sub_topic_elec_system)
        self.client.subscribe(self.sub_topic_engine_system)
        self.client.subscribe(self.sub_topic_warnning_system)
        self.client.subscribe(self.sub_topic_equipment)
        self.client.subscribe(self.sub_topic_command_echo)

        self.client.on_message = self.__on_recv_mqtt_msg

    def run(self):
        # mqtt客户端监听接收到的消息
        self.client.loop_forever()

    def _signal_event_init(self):
        self.sig_request_echo.connect(self._request_echo)  # 触发返回结果

    def _request_echo(self, echo_msg):
        """指令执行完成后返回信息"""
        # print(echo_msg)
        pass

    def publish_message(self, topic, msg_str):

        result = self.client.publish(topic, msg_str)
        # result: [0, 1]
        status = result[0]
        if 0 != status:
            logging.warning(f"Failed to send message to topic {topic}")

    # 遥控指令发送
    def send_request(self, send_cmd=None):
        try:
            if send_cmd is None:
                send_cmd = self.cmd_to_send
                msg_str = self.cmd_to_send_msg
                logging.info("遥控指令:" + msg_str)
            else:
                msg_str = get_send_cmd_msg(send_cmd)
                logging.info("遥控指令：" + msg_str)

            replay = QMessageBox.Yes

            # TODO:需要二次确认的命令在此进行拦截提示
            # 如果检测到要发送复位命令则弹窗提示
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_RESET:
                replay = QMessageBox.information(None, "警告", "是否确认复位？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            # 如果检测到在空中发送停车命令则弹窗提示
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_STOP \
                    and self.fcc_system['control_mode']['on_air'] == 1:
                replay = QMessageBox.information(None, "警告", "当前飞机在空中，是否确认停车？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)

            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_LINK_SEC_ANGLE:
                replay = QMessageBox.information(None, "警告", "姿态数据源切换可能导致问题，确认接入备惯姿态？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_LINK_MASTER_ANGLE:
                replay = QMessageBox.information(None, "警告", "姿态数据源切换可能导致问题，确认接入主惯姿态？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_LINK_MASTER_POS:
                replay = QMessageBox.information(None, "警告", "导航数据源切换可能导致问题，确认接入主惯位置？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_LINK_GPS_POS:
                replay = QMessageBox.information(None, "警告", "导航数据源切换可能导致问题，确认接入GNSS位置？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_LINK_GPS_ALT:
                replay = QMessageBox.information(None, "警告", "导航数据源切换可能导致问题，确认接入备惯位置？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            if send_cmd['isControlCmd'] == 1 \
                    and send_cmd['ControlCmd'] == COMMAND_CTRL_LINK_MASTER_ALT:
                replay = QMessageBox.information(None, "警告", "高度数据源切换可能导致问题，确认接入主惯高度？",
                                                 QMessageBox.No |
                                                 QMessageBox.Yes, QMessageBox.Yes)
            if replay == QMessageBox.Yes:
                # self.request_queue.put(send_cmd)
                # self.request_queue.put(send_cmd)
                self.request_queue.put(send_cmd)  # 连续发送三次命令，减小链路命令发送失败的概率

                # 向超时检测队列中增加数据，在定时器中进行回传数据检测
                # self.sig_back_cmd_monitor.emit(dict_send_cmd)
                self.timout_request_queue.put(send_cmd)

                # 触发指令发送更新信号
                self.sig_send_request.emit(msg_str)

        except Exception as e:
            logging.error(repr(e))

    def request_route_start(self, route_type=0):
        """
        开始航线查询
        :param route_type:航点类型 0-在线航点 1-滑行驶入航点 2-滑行驶出航点
        :return:
        """
        if self.send_route_enable == 1:
            logging.warning("正在上传航线，不响应查询指令")
        else:
            self.get_complete_dot_cnt = 0
            self.last_AdjustCmdData = -1
            self.request_route_type = route_type
            self.request_result = []  # 储存查询结果的队列
            # 开始查询

            self.request_online_route_data(1, self.request_route_type)

    def request_online_route_data(self, route_index=1, route_type=0):
        """
        查询指定index对应的航路点
        :param route_type: 航点类型 0-在线航点 1-滑行驶入航点 2-滑行驶出航点
        :param route_index: 从1开始计数
        :return:
        """
        if route_type == 0:
            # 查询第一个在线航点，开始查询逻辑
            self.pre_send_request_cmd(1, COMMAND_ADJUST_ROUTE_BACK, route_index)
        elif route_type == 1:
            # 查询第一个跑道驶入航点，开始查询逻辑
            self.pre_send_request_cmd(1, COMMAND_ADJUST_TAXI_IN_ROUTE_DOWNLOAD, route_index)
        elif route_type == 2:
            # 查询第一个跑道驶出航点，开始查询逻辑
            self.pre_send_request_cmd(1, COMMAND_ADJUST_TAXI_OUT_ROUTE_DOWNLOAD, route_index)

        self.send_request()

    def send_route_start(self, route_list: List[RouteInfo], route_type=0):
        """
        开始上传航线
        :param route_list: 待上传航线列表
        :param route_type: 航点类型 0-在线航点 1-滑行驶入航点 2-滑行驶出航点
        :return:
        """
        if self.send_route_enable == 1:
            logging.warning("正在上传航线，不响应")
            return

        # 初始化待上传航线数据
        self.send_route_enable = 1
        self.counter_upload_fail = 0
        self.list_route_to_send = route_list
        self.upload_route_type = route_type
        self.counter_send_dot_fail_retry = 0
        self._send_next_dot(self.upload_route_type)

    def _send_next_dot(self, dot_type=0):
        """
        从待上传航线钟取出一点进行上传
        :param dot_type: 航点类型 0-在线航点 1-滑行驶入航点 2-滑行驶出航点
        :return:
        """
        if dot_type == 0:
            # 将第一个点的上传命令添加到待发送队列,调用此函数前需确认前一个点已经删除
            cmd_param = {'on_way_dot': self.list_route_to_send[0].no,
                         'on_way_lon': self.list_route_to_send[0].lon,
                         'on_way_lat': self.list_route_to_send[0].lat,
                         'on_way_alt': self.list_route_to_send[0].alt,
                         'on_way_vxd': self.list_route_to_send[0].status << 12
                                       + self.list_route_to_send[0].parm}

            self.pre_send_request_cmd(1, COMMAND_ADJUST_ROUTE_SET, cmd_param)
            self.send_request()
        elif dot_type == 1:
            # 将第一个点的上传命令添加到待发送队列,调用此函数前需确认前一个点已经删除
            cmd_param = {
                "taxi_way_dot": self.list_route_to_send[0].no,  # 航点号
                "taxi_way_lon": self.list_route_to_send[0].lon,  # 航点经度
                "taxi_way_lat": self.list_route_to_send[0].lat,  # 航点纬度
                "taxi_way_radious": self.list_route_to_send[0].radious,  # 转弯半径
            }

            self.pre_send_request_cmd(1, COMMAND_ADJUST_TAXI_IN_ROUTE_UPLOAD, cmd_param)
            self.send_request()
        elif dot_type == 2:
            # 将第一个点的上传命令添加到待发送队列,调用此函数前需确认前一个点已经删除
            cmd_param = {
                "taxi_way_dot": self.list_route_to_send[0].no,  # 航点号
                "taxi_way_lon": self.list_route_to_send[0].lon,  # 航点经度
                "taxi_way_lat": self.list_route_to_send[0].lat,  # 航点纬度
                "taxi_way_radious": self.list_route_to_send[0].radious,  # 转弯半径
            }

            self.pre_send_request_cmd(1, COMMAND_ADJUST_TAXI_OUT_ROUTE_UPLOAD, cmd_param)
            self.send_request()


datahandler = DataHandler()
